//
// Created by lijia on 2021/1/12.
//

#include "gps.h"
#include <string.h>
#include <math.h>
#include "system.h"
#include "qfplib-m3.h"

#define sq(x) (qfp_fmul(x, x))

#define GPS_BUFFER_SIZE 4096

static GpsData data;
static uint8_t buf[GPS_BUFFER_SIZE], rBuf[16], sentenceBuf[128];
static size_t readPos = 0, writePos = 0;
static uint8_t state = 0;
static volatile uint8_t wrapped = 0;
static DMA_HandleTypeDef *hwnd;
static UART_HandleTypeDef *huart;

static void wrapped_copy(uint8_t *out, const uint8_t *in, size_t offset, size_t length, size_t wrap) {
	size_t i = offset, count = 0;
	while (count < length) {
		out[count] = in[i];
		if (++i >= wrap) {
			i = 0;
		}
		++count;
	}
}

static size_t wrapped_copy_until_comma(uint8_t *out, const uint8_t *in, size_t offset, size_t maxLength, size_t wrap) {
	size_t i = offset, count = 0;
	while (count < maxLength) {
		if (in[i] == ',') {
			out[count] = 0;
			break;
		}
		out[count] = in[i];
		if (++i >= wrap) {
			i = 0;
		}
		++count;
	}
	return count;
}

static uint8_t has_comma_pending(const uint8_t *in, size_t offset, size_t maxLength, size_t wrap) {
	size_t i = offset, count = 0;
	while (count < maxLength) {
		if (in[i] == ',') {
			return 1;
		}
		if (++i >= wrap) {
			i = 0;
		}
		++count;
	}
	return 0;
}

static void read_position_forward(size_t count) {
	readPos += count;
	if (readPos >= GPS_BUFFER_SIZE) {
		readPos -= GPS_BUFFER_SIZE;
	}
}

static size_t available_data_count() {
	return writePos - readPos;
}

int32_t parse_latitude(const uint8_t *x) {
	int32_t out;

	out = (x[0] - '0') * 10000000;
	out += (x[1] - '0') * 1000000;
	out += (x[2] - '0') * 100000;
	out += (x[3] - '0') * 10000;
	out += (x[5] - '0') * 1000;
	out += (x[6] - '0') * 100;
	out += (x[7] - '0') * 10;
	out += (x[8] - '0');

	if (x[10] != 'N') {
		out = -out;
	}

	return out;
}

int32_t parse_direct_latitude(const uint8_t *x) {
	int32_t out;

	out = (x[0] - '0') * 10000000;
	out += (x[1] - '0') * 1000000;
	out += (x[2] - '0') * 100000;
	out += (x[3] - '0') * 10000;
	out += (x[4] - '0') * 1000;
	out += (x[5] - '0') * 100;
	out += (x[6] - '0') * 10;
	out += (x[7] - '0');

	return out;
}

int32_t parse_longitude(const uint8_t *x) {
	int32_t out;

	out = (x[0] - '0') * 100000000;
	out += (x[1] - '0') * 10000000;
	out += (x[2] - '0') * 1000000;
	out += (x[3] - '0') * 100000;
	out += (x[4] - '0') * 10000;
	out += (x[6] - '0') * 1000;
	out += (x[7] - '0') * 100;
	out += (x[8] - '0') * 10;
	out += (x[9] - '0');

	if (x[11] != 'E') {
		out = -out;
	}

	return out;
}

int32_t parse_direct_longitude(const uint8_t *x) {
	int32_t out;

	out = (x[0] - '0') * 100000000;
	out += (x[1] - '0') * 10000000;
	out += (x[2] - '0') * 1000000;
	out += (x[3] - '0') * 100000;
	out += (x[4] - '0') * 10000;
	out += (x[5] - '0') * 1000;
	out += (x[6] - '0') * 100;
	out += (x[7] - '0') * 10;
	out += (x[8] - '0');

	return out;
}

static int32_t parse_elevation(const uint8_t *x) {
	uint8_t length = strlen((const char *) x); // in x.1 format, maybe
	int32_t weight = 10;
	int32_t out = 0;
	out = (x[length - 1] - '0');
	for (uint8_t i = 2; i < length; i++) {
		char c = x[length - i - 1];
		out += weight * (c - '0');
		weight *= 10;
	}
	return out;
}

void gps_init(UART_HandleTypeDef *handle, DMA_HandleTypeDef *h2) {
	hwnd = h2;
	huart = handle;
	state = 0;
	data.frame = 0;
	HAL_UART_Receive_DMA(handle, buf, GPS_BUFFER_SIZE);
}

void gps_tick() {
	if (!hwnd) {
		return;
	}
	HAL_DMA_Abort(hwnd);
	size_t tmp = 0;
	writePos = GPS_BUFFER_SIZE - __HAL_DMA_GET_COUNTER(hwnd);
	switch (state) {
		case 0: // find '$'
		__gps_parser_parse_preamble:
			if (available_data_count() < 1) {
				state = 0;
				break;
			}
			while (buf[readPos] != '$') {
				if (++readPos >= writePos) {
					readPos = 0;
					state = 0;
					goto __gps_parser_done;
				}
			}
			read_position_forward(1);
			state = 1;
			goto __gps_parser_parse_command;
			break;
		case 1: // the next part will be commands
		__gps_parser_parse_command:
			// we would only be interested at __GGA commands
			// so for any other stuff that will be a skip to next command
			if (available_data_count() < 5) {
				break;
			}
			wrapped_copy(rBuf, buf, readPos, 5, GPS_BUFFER_SIZE);
			if (rBuf[2] == 'G' && rBuf[3] == 'G' && rBuf[4] == 'A') {
				read_position_forward(6); // skip through the comma
				state = 2;
				goto __gps_parser_parse_time;
			} else {
				state = 0;
				goto __gps_parser_parse_preamble;
			}
			break;
		case 2: // parse time, actually we don't care and it will be discarded
		__gps_parser_parse_time:
			if (available_data_count() >= 11) {
				read_position_forward(11);
				state = 3;
				goto __gps_parser_parse_latitude;
			}
			break;
		case 3: // parse latitude
		__gps_parser_parse_latitude:
			if (available_data_count() >= 12) {
				wrapped_copy(rBuf, buf, readPos, 11, GPS_BUFFER_SIZE);
				data.lat = gps_dm6_to_ddd(parse_latitude(rBuf));
				read_position_forward(12);
				state = 4;
				goto __gps_parser_parse_longitude;
			}
			break;
		case 4: // parse longitude
		__gps_parser_parse_longitude:
			if (available_data_count() >= 13) {
				wrapped_copy(rBuf, buf, readPos, 12, GPS_BUFFER_SIZE);
				data.lng = gps_dm6_to_ddd(parse_longitude(rBuf));
				read_position_forward(13);
				state = 5;
				goto __gps_parser_parse_fix;
			}
			break;
		case 5: // gps fix state
		__gps_parser_parse_fix:
			if (available_data_count() >= 2) {
				data.fixState = buf[readPos] - '0';
				read_position_forward(2);
				state = 6;
				goto __gps_parser_parse_sat_count;
			}
			break;
		case 6: // sat count
		__gps_parser_parse_sat_count:
			if (available_data_count() >= 3) {
				wrapped_copy(rBuf, buf, readPos, 2, GPS_BUFFER_SIZE);
				data.satCount = 10 * (rBuf[0] - '0');
				data.satCount += (rBuf[1] - '0');
				state = 7;
				read_position_forward(3);
				goto __gps_parser_parse_precision;
			}
			break;
		case 7: // precision
		__gps_parser_parse_precision:
			if (has_comma_pending(buf, readPos, available_data_count(), GPS_BUFFER_SIZE)) {
				tmp = wrapped_copy_until_comma(rBuf, buf, readPos, available_data_count(), GPS_BUFFER_SIZE);
				// TODO: parse
				state = 8;
				read_position_forward(tmp + 1);
				goto __gps_parser_parse_elevation;
			}
			break;
		case 8: // elevation
		__gps_parser_parse_elevation:
			if (has_comma_pending(buf, readPos, available_data_count(), GPS_BUFFER_SIZE)) {
				tmp = wrapped_copy_until_comma(rBuf, buf, readPos, available_data_count(), GPS_BUFFER_SIZE);
				data.elevation = parse_elevation(rBuf);
				state = 100;
				// other data are discarded
				goto __gps_parser_fast_forward;
			}
			break;
		case 100: // forward to either end of data or \r\n
		__gps_parser_fast_forward:
			data.frame++;
			state = 0;
			readPos = 0;
			break;
		default:
			state = 0;
			goto __gps_parser_parse_preamble;
			break;
	}
	if (data.frame > 0) {
		if (!system_is_module_ready(MODULE_GPS)) {
			system_mark_module_ready(MODULE_GPS);
		}
	}
	__gps_parser_done:
	HAL_UART_Receive_DMA(huart, buf, GPS_BUFFER_SIZE);
}

const GpsData *gps_get_last_frame() {
	return &data;
}

float gps_dot4_to_deg(int32_t x) {
	// x is s4.4; 4124.8963; 41248963
	int32_t v = x;
	if (x < 0) {
		v = -v;
	}
	uint32_t seconds = v % 1000000;
	uint32_t degrees = v / 1000000;
	float out = (float) degrees;
	float fSeconds = (float) seconds;
	fSeconds /= 10000.f;
	out += (fSeconds / 60.f);
	return x < 0 ? -out : out;
}

int32_t gps_dm6_to_ddd(int32_t x) {
	int32_t minutePart = x % 1000000;
	int32_t degreePart = x / 1000000;
	int32_t decimalPart = minutePart * 10 / 6;
	return degreePart * 1000000 + decimalPart;
}

void gps_angle(int32_t* lat1, int32_t* lon1, int32_t* lat2, int32_t* lon2, int32_t* bearing) {
	float GPS_scaleLonDown = qfp_fcos(*lat2 * 1.0e-7f * 0.01745329251f);
	int32_t off_x = *lon2 - *lon1;
	int32_t off_y = (*lat2 - *lat1) / GPS_scaleLonDown;

	*bearing = 9000 + qfp_fatan2((float)-off_y, off_x) * 5729.57795f;      //Convert the output redians to 100xdeg
	if (*bearing < 0) *bearing += 36000;
}

void gps_distance_cm(int32_t* lat1, int32_t* lon1, int32_t* lat2, int32_t* lon2, uint32_t* dist) {
	float GPS_scaleLonDown = qfp_fcos(*lat2 * 1.0e-7f * 0.01745329251f);
	float dLat = (float)(*lat2 - *lat1);                                    // difference of latitude in 1/10 000 000 degrees
	float dLon = (float)(*lon2 - *lon1) * GPS_scaleLonDown; //x
	*dist = qfp_fsqrt(sq(dLat) + sq(dLon)) * 1.11318845f;
}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *_) {
	// invoked when reached the end of rx buffer
	// but might not actually done
	//wrapped = 1;
	//HAL_UART_Receive_DMA(_, buf, GPS_BUFFER_SIZE);
}
